/*
 * Test program for the servos: go to 90°, disable the motors, then go to 0°
 */
# include "init_systems.h"
# include "log.h"
# include <rrcon.h>
# include <time_tools.h>

# define MAXSPEED 1023

using namespace kthrobot;
using namespace std;

int main (int argc, char **argv) {
    // Init
    Init::RegisterCommon();
    RRCon::Register();
    if (!Init::Systems(argc, argv)) return 1;
    srand(time(NULL));

    // Initializing the servo
    Servo& servo = RRCon::servo(0);

    // Moving to 90°
    Log::Write("Moving to 90°");
    servo.SetAngle(90);
    TimeTools::WaitSecs(3);
    // Disabling the motor
    Log::Write("Disabling motor");
    servo.Disable();
    TimeTools::WaitSecs(3);
    // Moving to 0°
    Log::Write("Moving to 0°");
    servo.Enable();
    servo.SetAngle(0);
    TimeTools::WaitSecs(3);

    // Shutdown
    Init::Shutdown();
    return 0;
}
